

<!DOCTYPE html>
<!--[if IE 8]><html class="no-js lt-ie9" lang="en" > <![endif]-->
<!--[if gt IE 8]><!--> <html class="no-js" lang="en" > <!--<![endif]-->
<head>
  <meta charset="utf-8">
  
  <meta name="viewport" content="width=device-width, initial-scale=1.0">
  
  <title>packnet_sfm.datasets.kitti_dataset_utils &mdash; PackNet-SfM 1.0 documentation</title>
  

  
  
  
  

  
  <script type="text/javascript" src="../../../_static/js/modernizr.min.js"></script>
  
    
      <script type="text/javascript" id="documentation_options" data-url_root="../../../" src="../../../_static/documentation_options.js"></script>
        <script src="../../../_static/jquery.js"></script>
        <script src="../../../_static/underscore.js"></script>
        <script src="../../../_static/doctools.js"></script>
        <script src="../../../_static/language_data.js"></script>
    
    <script type="text/javascript" src="../../../_static/js/theme.js"></script>

    

  
  <link rel="stylesheet" href="../../../_static/css/theme.css" type="text/css" />
  <link rel="stylesheet" href="../../../_static/pygments.css" type="text/css" />
  <link rel="stylesheet" href="../../../_static/custom.css" type="text/css" />
    <link rel="index" title="Index" href="../../../genindex.html" />
    <link rel="search" title="Search" href="../../../search.html" /> 
</head>

<body class="wy-body-for-nav">

   
  <div class="wy-grid-for-nav">
    
    <nav data-toggle="wy-nav-shift" class="wy-nav-side">
      <div class="wy-side-scroll">
        <div class="wy-side-nav-search" >
          

          
            <a href="../../../index.html">
          

          
            
            <img src="../../../_static/logo.png" class="logo" alt="Logo"/>
          
          </a>

          
            
            
          

          
<div role="search">
  <form id="rtd-search-form" class="wy-form" action="../../../search.html" method="get">
    <input type="text" name="q" placeholder="Search docs" />
    <input type="hidden" name="check_keywords" value="yes" />
    <input type="hidden" name="area" value="default" />
  </form>
</div>

          
        </div>

        <div class="wy-menu wy-menu-vertical" data-spy="affix" role="navigation" aria-label="main navigation">
          
            
            
              
            
            
              <p class="caption"><span class="caption-text">Contents</span></p>
<ul>
<li class="toctree-l1"><a class="reference internal" href="../../../configs/configs.html">Configs</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../scripts/scripts.html">Scripts</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../trainers/trainers.html">Trainers</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../datasets/datasets.html">Datasets</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../models/models.html">Models</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../networks/networks.html">Networks</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../losses/losses.html">Losses</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../loggers/loggers.html">Loggers</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../geometry/geometry.html">Geometry</a></li>
<li class="toctree-l1"><a class="reference internal" href="../../../utils/utils.html">Utils</a></li>
</ul>
<p class="caption"><span class="caption-text">Contact</span></p>
<ul>
<li class="toctree-l1"><a class="reference external" href="https://tri.global">Toyota Research Institute</a></li>
<li class="toctree-l1"><a class="reference external" href="https://github.com/TRI-ML/packnet-sfm">PackNet-SfM GitHub</a></li>
<li class="toctree-l1"><a class="reference external" href="https://github.com/TRI-ML/DDAD">DDAD GitHub</a></li>
</ul>

            
          
        </div>
      </div>
    </nav>

    <section data-toggle="wy-nav-shift" class="wy-nav-content-wrap">

      
      <nav class="wy-nav-top" aria-label="top navigation">
        
          <i data-toggle="wy-nav-top" class="fa fa-bars"></i>
          <a href="../../../index.html">PackNet-SfM</a>
        
      </nav>


      <div class="wy-nav-content">
        
        <div class="rst-content">
        
          















<div role="navigation" aria-label="breadcrumbs navigation">

  <ul class="wy-breadcrumbs">
    
      <li><a href="../../../index.html">Docs</a> &raquo;</li>
        
          <li><a href="../../index.html">Module code</a> &raquo;</li>
        
      <li>packnet_sfm.datasets.kitti_dataset_utils</li>
    
    
      <li class="wy-breadcrumbs-aside">
        
      </li>
    
  </ul>

  
  <hr/>
</div>
          <div role="main" class="document" itemscope="itemscope" itemtype="http://schema.org/Article">
           <div itemprop="articleBody">
            
  <h1>Source code for packnet_sfm.datasets.kitti_dataset_utils</h1><div class="highlight"><pre>
<span></span><span class="sd">&quot;&quot;&quot;Provides helper methods for loading and parsing KITTI data.&quot;&quot;&quot;</span>

<span class="kn">from</span> <span class="nn">collections</span> <span class="kn">import</span> <span class="n">namedtuple</span>

<span class="kn">import</span> <span class="nn">numpy</span> <span class="k">as</span> <span class="nn">np</span>

<span class="n">__author__</span> <span class="o">=</span> <span class="s2">&quot;Lee Clement&quot;</span>
<span class="n">__email__</span> <span class="o">=</span> <span class="s2">&quot;lee.clement@robotics.utias.utoronto.ca&quot;</span>

<span class="c1"># Per dataformat.txt</span>
<span class="n">OxtsPacket</span> <span class="o">=</span> <span class="n">namedtuple</span><span class="p">(</span><span class="s1">&#39;OxtsPacket&#39;</span><span class="p">,</span>
                        <span class="s1">&#39;lat, lon, alt, &#39;</span> <span class="o">+</span>
                        <span class="s1">&#39;roll, pitch, yaw, &#39;</span> <span class="o">+</span>
                        <span class="s1">&#39;vn, ve, vf, vl, vu, &#39;</span> <span class="o">+</span>
                        <span class="s1">&#39;ax, ay, az, af, al, au, &#39;</span> <span class="o">+</span>
                        <span class="s1">&#39;wx, wy, wz, wf, wl, wu, &#39;</span> <span class="o">+</span>
                        <span class="s1">&#39;pos_accuracy, vel_accuracy, &#39;</span> <span class="o">+</span>
                        <span class="s1">&#39;navstat, numsats, &#39;</span> <span class="o">+</span>
                        <span class="s1">&#39;posmode, velmode, orimode&#39;</span><span class="p">)</span>

<span class="c1"># Bundle into an easy-to-access structure</span>
<span class="n">OxtsData</span> <span class="o">=</span> <span class="n">namedtuple</span><span class="p">(</span><span class="s1">&#39;OxtsData&#39;</span><span class="p">,</span> <span class="s1">&#39;packet, T_w_imu&#39;</span><span class="p">)</span>


<div class="viewcode-block" id="rotx"><a class="viewcode-back" href="../../../datasets/datasets.KITTIDataset_utils.html#packnet_sfm.datasets.kitti_dataset_utils.rotx">[docs]</a><span class="k">def</span> <span class="nf">rotx</span><span class="p">(</span><span class="n">t</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Rotation about the x-axis</span>

<span class="sd">    Parameters</span>
<span class="sd">    ----------</span>
<span class="sd">    t : float</span>
<span class="sd">        Theta angle</span>

<span class="sd">    Returns</span>
<span class="sd">    -------</span>
<span class="sd">    matrix : np.array [3,3]</span>
<span class="sd">        Rotation matrix</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">c</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">t</span><span class="p">)</span>
    <span class="n">s</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">t</span><span class="p">)</span>
    <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([[</span><span class="mi">1</span><span class="p">,</span>  <span class="mi">0</span><span class="p">,</span>  <span class="mi">0</span><span class="p">],</span>
                     <span class="p">[</span><span class="mi">0</span><span class="p">,</span>  <span class="n">c</span><span class="p">,</span> <span class="o">-</span><span class="n">s</span><span class="p">],</span>
                     <span class="p">[</span><span class="mi">0</span><span class="p">,</span>  <span class="n">s</span><span class="p">,</span>  <span class="n">c</span><span class="p">]])</span></div>


<div class="viewcode-block" id="roty"><a class="viewcode-back" href="../../../datasets/datasets.KITTIDataset_utils.html#packnet_sfm.datasets.kitti_dataset_utils.roty">[docs]</a><span class="k">def</span> <span class="nf">roty</span><span class="p">(</span><span class="n">t</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Rotation about the y-axis</span>

<span class="sd">    Parameters</span>
<span class="sd">    ----------</span>
<span class="sd">    t : float</span>
<span class="sd">        Theta angle</span>

<span class="sd">    Returns</span>
<span class="sd">    -------</span>
<span class="sd">    matrix : np.array [3,3]</span>
<span class="sd">        Rotation matrix</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">c</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">t</span><span class="p">)</span>
    <span class="n">s</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">t</span><span class="p">)</span>
    <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([[</span><span class="n">c</span><span class="p">,</span>  <span class="mi">0</span><span class="p">,</span>  <span class="n">s</span><span class="p">],</span>
                     <span class="p">[</span><span class="mi">0</span><span class="p">,</span>  <span class="mi">1</span><span class="p">,</span>  <span class="mi">0</span><span class="p">],</span>
                     <span class="p">[</span><span class="o">-</span><span class="n">s</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span>  <span class="n">c</span><span class="p">]])</span></div>


<div class="viewcode-block" id="rotz"><a class="viewcode-back" href="../../../datasets/datasets.KITTIDataset_utils.html#packnet_sfm.datasets.kitti_dataset_utils.rotz">[docs]</a><span class="k">def</span> <span class="nf">rotz</span><span class="p">(</span><span class="n">t</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Rotation about the z-axis</span>

<span class="sd">    Parameters</span>
<span class="sd">    ----------</span>
<span class="sd">    t : float</span>
<span class="sd">        Theta angle</span>

<span class="sd">    Returns</span>
<span class="sd">    -------</span>
<span class="sd">    matrix : np.array [3,3]</span>
<span class="sd">        Rotation matrix</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">c</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">t</span><span class="p">)</span>
    <span class="n">s</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">sin</span><span class="p">(</span><span class="n">t</span><span class="p">)</span>
    <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([[</span><span class="n">c</span><span class="p">,</span> <span class="o">-</span><span class="n">s</span><span class="p">,</span>  <span class="mi">0</span><span class="p">],</span>
                     <span class="p">[</span><span class="n">s</span><span class="p">,</span>  <span class="n">c</span><span class="p">,</span>  <span class="mi">0</span><span class="p">],</span>
                     <span class="p">[</span><span class="mi">0</span><span class="p">,</span>  <span class="mi">0</span><span class="p">,</span>  <span class="mi">1</span><span class="p">]])</span></div>


<div class="viewcode-block" id="transform_from_rot_trans"><a class="viewcode-back" href="../../../datasets/datasets.KITTIDataset_utils.html#packnet_sfm.datasets.kitti_dataset_utils.transform_from_rot_trans">[docs]</a><span class="k">def</span> <span class="nf">transform_from_rot_trans</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">t</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Transformation matrix from rotation matrix and translation vector.</span>

<span class="sd">    Parameters</span>
<span class="sd">    ----------</span>
<span class="sd">    R : np.array [3,3]</span>
<span class="sd">        Rotation matrix</span>
<span class="sd">    t : np.array [3]</span>
<span class="sd">        translation vector</span>

<span class="sd">    Returns</span>
<span class="sd">    -------</span>
<span class="sd">    matrix : np.array [4,4]</span>
<span class="sd">        Transformation matrix</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">R</span> <span class="o">=</span> <span class="n">R</span><span class="o">.</span><span class="n">reshape</span><span class="p">(</span><span class="mi">3</span><span class="p">,</span> <span class="mi">3</span><span class="p">)</span>
    <span class="n">t</span> <span class="o">=</span> <span class="n">t</span><span class="o">.</span><span class="n">reshape</span><span class="p">(</span><span class="mi">3</span><span class="p">,</span> <span class="mi">1</span><span class="p">)</span>
    <span class="k">return</span> <span class="n">np</span><span class="o">.</span><span class="n">vstack</span><span class="p">((</span><span class="n">np</span><span class="o">.</span><span class="n">hstack</span><span class="p">([</span><span class="n">R</span><span class="p">,</span> <span class="n">t</span><span class="p">]),</span> <span class="p">[</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">]))</span></div>


<div class="viewcode-block" id="read_calib_file"><a class="viewcode-back" href="../../../datasets/datasets.KITTIDataset_utils.html#packnet_sfm.datasets.kitti_dataset_utils.read_calib_file">[docs]</a><span class="k">def</span> <span class="nf">read_calib_file</span><span class="p">(</span><span class="n">filepath</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Read in a calibration file and parse into a dictionary</span>

<span class="sd">    Parameters</span>
<span class="sd">    ----------</span>
<span class="sd">    filepath : str</span>
<span class="sd">        File path to read from</span>

<span class="sd">    Returns</span>
<span class="sd">    -------</span>
<span class="sd">    calib : dict</span>
<span class="sd">        Dictionary with calibration values</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">data</span> <span class="o">=</span> <span class="p">{}</span>

    <span class="k">with</span> <span class="nb">open</span><span class="p">(</span><span class="n">filepath</span><span class="p">,</span> <span class="s1">&#39;r&#39;</span><span class="p">)</span> <span class="k">as</span> <span class="n">f</span><span class="p">:</span>
        <span class="k">for</span> <span class="n">line</span> <span class="ow">in</span> <span class="n">f</span><span class="o">.</span><span class="n">readlines</span><span class="p">():</span>
            <span class="n">key</span><span class="p">,</span> <span class="n">value</span> <span class="o">=</span> <span class="n">line</span><span class="o">.</span><span class="n">split</span><span class="p">(</span><span class="s1">&#39;:&#39;</span><span class="p">,</span> <span class="mi">1</span><span class="p">)</span>
            <span class="c1"># The only non-float values in these files are dates, which</span>
            <span class="c1"># we don&#39;t care about anyway</span>
            <span class="k">try</span><span class="p">:</span>
                <span class="n">data</span><span class="p">[</span><span class="n">key</span><span class="p">]</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="nb">float</span><span class="p">(</span><span class="n">x</span><span class="p">)</span> <span class="k">for</span> <span class="n">x</span> <span class="ow">in</span> <span class="n">value</span><span class="o">.</span><span class="n">split</span><span class="p">()])</span>
            <span class="k">except</span> <span class="ne">ValueError</span><span class="p">:</span>
                <span class="k">pass</span>

    <span class="k">return</span> <span class="n">data</span></div>


<div class="viewcode-block" id="pose_from_oxts_packet"><a class="viewcode-back" href="../../../datasets/datasets.KITTIDataset_utils.html#packnet_sfm.datasets.kitti_dataset_utils.pose_from_oxts_packet">[docs]</a><span class="k">def</span> <span class="nf">pose_from_oxts_packet</span><span class="p">(</span><span class="n">raw_data</span><span class="p">,</span> <span class="n">scale</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Helper method to compute a SE(3) pose matrix from an OXTS packet</span>

<span class="sd">    Parameters</span>
<span class="sd">    ----------</span>
<span class="sd">    raw_data : dict</span>
<span class="sd">        Oxts data to read from</span>
<span class="sd">    scale : float</span>
<span class="sd">        Oxts scale</span>

<span class="sd">    Returns</span>
<span class="sd">    -------</span>
<span class="sd">    R : np.array [3,3]</span>
<span class="sd">        Rotation matrix</span>
<span class="sd">    t : np.array [3]</span>
<span class="sd">        Translation vector</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="n">packet</span> <span class="o">=</span> <span class="n">OxtsPacket</span><span class="p">(</span><span class="o">*</span><span class="n">raw_data</span><span class="p">)</span>
    <span class="n">er</span> <span class="o">=</span> <span class="mf">6378137.</span>  <span class="c1"># earth radius (approx.) in meters</span>

    <span class="c1"># Use a Mercator projection to get the translation vector</span>
    <span class="n">tx</span> <span class="o">=</span> <span class="n">scale</span> <span class="o">*</span> <span class="n">packet</span><span class="o">.</span><span class="n">lon</span> <span class="o">*</span> <span class="n">np</span><span class="o">.</span><span class="n">pi</span> <span class="o">*</span> <span class="n">er</span> <span class="o">/</span> <span class="mf">180.</span>
    <span class="n">ty</span> <span class="o">=</span> <span class="n">scale</span> <span class="o">*</span> <span class="n">er</span> <span class="o">*</span> \
        <span class="n">np</span><span class="o">.</span><span class="n">log</span><span class="p">(</span><span class="n">np</span><span class="o">.</span><span class="n">tan</span><span class="p">((</span><span class="mf">90.</span> <span class="o">+</span> <span class="n">packet</span><span class="o">.</span><span class="n">lat</span><span class="p">)</span> <span class="o">*</span> <span class="n">np</span><span class="o">.</span><span class="n">pi</span> <span class="o">/</span> <span class="mf">360.</span><span class="p">))</span>
    <span class="n">tz</span> <span class="o">=</span> <span class="n">packet</span><span class="o">.</span><span class="n">alt</span>
    <span class="n">t</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">array</span><span class="p">([</span><span class="n">tx</span><span class="p">,</span> <span class="n">ty</span><span class="p">,</span> <span class="n">tz</span><span class="p">])</span>

    <span class="c1"># Use the Euler angles to get the rotation matrix</span>
    <span class="n">Rx</span> <span class="o">=</span> <span class="n">rotx</span><span class="p">(</span><span class="n">packet</span><span class="o">.</span><span class="n">roll</span><span class="p">)</span>
    <span class="n">Ry</span> <span class="o">=</span> <span class="n">roty</span><span class="p">(</span><span class="n">packet</span><span class="o">.</span><span class="n">pitch</span><span class="p">)</span>
    <span class="n">Rz</span> <span class="o">=</span> <span class="n">rotz</span><span class="p">(</span><span class="n">packet</span><span class="o">.</span><span class="n">yaw</span><span class="p">)</span>
    <span class="n">R</span> <span class="o">=</span> <span class="n">Rz</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">Ry</span><span class="o">.</span><span class="n">dot</span><span class="p">(</span><span class="n">Rx</span><span class="p">))</span>

    <span class="c1"># Combine the translation and rotation into a homogeneous transform</span>
    <span class="k">return</span> <span class="n">R</span><span class="p">,</span> <span class="n">t</span></div>


<div class="viewcode-block" id="load_oxts_packets_and_poses"><a class="viewcode-back" href="../../../datasets/datasets.KITTIDataset_utils.html#packnet_sfm.datasets.kitti_dataset_utils.load_oxts_packets_and_poses">[docs]</a><span class="k">def</span> <span class="nf">load_oxts_packets_and_poses</span><span class="p">(</span><span class="n">oxts_files</span><span class="p">):</span>
    <span class="sd">&quot;&quot;&quot;</span>
<span class="sd">    Generator to read OXTS ground truth data.</span>
<span class="sd">    Poses are given in an East-North-Up coordinate system</span>
<span class="sd">    whose origin is the first GPS position.</span>

<span class="sd">    Parameters</span>
<span class="sd">    ----------</span>
<span class="sd">    oxts_files : list of str</span>
<span class="sd">        List of oxts files to read from</span>

<span class="sd">    Returns</span>
<span class="sd">    -------</span>
<span class="sd">    oxts : list of dict</span>
<span class="sd">        List of oxts ground-truth data</span>
<span class="sd">    &quot;&quot;&quot;</span>
    <span class="c1"># Scale for Mercator projection (from first lat value)</span>
    <span class="n">scale</span> <span class="o">=</span> <span class="kc">None</span>
    <span class="c1"># Origin of the global coordinate system (first GPS position)</span>
    <span class="n">origin</span> <span class="o">=</span> <span class="kc">None</span>

    <span class="n">oxts</span> <span class="o">=</span> <span class="p">[]</span>

    <span class="k">for</span> <span class="n">filename</span> <span class="ow">in</span> <span class="n">oxts_files</span><span class="p">:</span>
        <span class="k">with</span> <span class="nb">open</span><span class="p">(</span><span class="n">filename</span><span class="p">,</span> <span class="s1">&#39;r&#39;</span><span class="p">)</span> <span class="k">as</span> <span class="n">f</span><span class="p">:</span>
            <span class="k">for</span> <span class="n">line</span> <span class="ow">in</span> <span class="n">f</span><span class="o">.</span><span class="n">readlines</span><span class="p">():</span>
                <span class="n">line</span> <span class="o">=</span> <span class="n">line</span><span class="o">.</span><span class="n">split</span><span class="p">()</span>
                <span class="c1"># Last five entries are flags and counts</span>
                <span class="n">line</span><span class="p">[:</span><span class="o">-</span><span class="mi">5</span><span class="p">]</span> <span class="o">=</span> <span class="p">[</span><span class="nb">float</span><span class="p">(</span><span class="n">x</span><span class="p">)</span> <span class="k">for</span> <span class="n">x</span> <span class="ow">in</span> <span class="n">line</span><span class="p">[:</span><span class="o">-</span><span class="mi">5</span><span class="p">]]</span>
                <span class="n">line</span><span class="p">[</span><span class="o">-</span><span class="mi">5</span><span class="p">:]</span> <span class="o">=</span> <span class="p">[</span><span class="nb">int</span><span class="p">(</span><span class="nb">float</span><span class="p">(</span><span class="n">x</span><span class="p">))</span> <span class="k">for</span> <span class="n">x</span> <span class="ow">in</span> <span class="n">line</span><span class="p">[</span><span class="o">-</span><span class="mi">5</span><span class="p">:]]</span>

                <span class="n">packet</span> <span class="o">=</span> <span class="n">OxtsPacket</span><span class="p">(</span><span class="o">*</span><span class="n">line</span><span class="p">)</span>

                <span class="k">if</span> <span class="n">scale</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
                    <span class="n">scale</span> <span class="o">=</span> <span class="n">np</span><span class="o">.</span><span class="n">cos</span><span class="p">(</span><span class="n">packet</span><span class="o">.</span><span class="n">lat</span> <span class="o">*</span> <span class="n">np</span><span class="o">.</span><span class="n">pi</span> <span class="o">/</span> <span class="mf">180.</span><span class="p">)</span>

                <span class="n">R</span><span class="p">,</span> <span class="n">t</span> <span class="o">=</span> <span class="n">pose_from_oxts_packet</span><span class="p">(</span><span class="n">packet</span><span class="p">,</span> <span class="n">scale</span><span class="p">)</span>

                <span class="k">if</span> <span class="n">origin</span> <span class="ow">is</span> <span class="kc">None</span><span class="p">:</span>
                    <span class="n">origin</span> <span class="o">=</span> <span class="n">t</span>

                <span class="n">T_w_imu</span> <span class="o">=</span> <span class="n">transform_from_rot_trans</span><span class="p">(</span><span class="n">R</span><span class="p">,</span> <span class="n">t</span> <span class="o">-</span> <span class="n">origin</span><span class="p">)</span>

                <span class="n">oxts</span><span class="o">.</span><span class="n">append</span><span class="p">(</span><span class="n">OxtsData</span><span class="p">(</span><span class="n">packet</span><span class="p">,</span> <span class="n">T_w_imu</span><span class="p">))</span>

    <span class="k">return</span> <span class="n">oxts</span></div>


</pre></div>

           </div>
           
          </div>
          <footer>
  

  <hr/>

  <div role="contentinfo">
    <p>
        &copy; Copyright 2020, Toyota Research Institute (TRI)

    </p>
  </div>
  Built with <a href="http://sphinx-doc.org/">Sphinx</a> using a <a href="https://github.com/rtfd/sphinx_rtd_theme">theme</a> provided by <a href="https://readthedocs.org">Read the Docs</a>. 

</footer>

        </div>
      </div>

    </section>

  </div>
  


  <script type="text/javascript">
      jQuery(function () {
          SphinxRtdTheme.Navigation.enable(false);
      });
  </script>

  
  
    
   

</body>
</html>